/**
  *************************(C) COPYRIGHT 2022 LionHeart*************************
  * @file       fall_down_task.c
  * @brief      升降任务
  * @note       
  * @history
  *  Version    Date                 Author          Modification
  *  V1.0.0     2021/12/20 16:01     yanfubing       1. done
  *  V1.0.1     2022/5/12            yanfubing       optimize fall down 
  @verbatim
  ==============================================================================
1.准备升降架
  warnning:fall_down_motor==go_down_motor
  ==============================================================================
  @endverbatim
  *************************(C) COPYRIGHT 2022 LionHeart*************************
  */   
#include "fall_down_task.h"	
#include "cmsis_os.h"
#include "arm_math.h"
#include "bsp_rc.h"
#include "bsp_buzzer.h"
#include "detect_task.h"
#include "main.h"
#include "rescue_IDcard_task.h"

fall_down_control_t fall_down_move;
int8_t fall_speed; //升降由CAN信号接收的里程速度
	
/**
	* @brief          上升下降初始化
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void fall_down_Init(fall_down_control_t* engineering_move_state);

/**
 * @brief 升降的里程
 * @param engineering_move_state 
 */
static void fall_down_mileage(fall_down_control_t* engineering_move_state);

/**
	* @brief          上升下降的执行
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void fall_down_set_state(fall_down_control_t* engineering_move_state);

/***************设置PID给每个电机电流*******************/
static void PID_realize(void);	

/**
 * @brief 升降任务
 * @param pvParameters 
 */
void fall_down_task(void const* pvParameters)
{                  
	      /**********等待一段时间********/
	  vTaskDelay(fall_down_task_INIT_TIME*2);
    fall_down_Init(&fall_down_move);
    while(toe_is_error(fall_down_MOTOR1_TOE) || toe_is_error(fall_down_MOTOR2_TOE))  
	{
		vTaskDelay(fall_down_task_INIT_TIME);
	}	
    while (1)
  {
		//延时一段时间
	   vTaskDelay(fall_down_task_INIT_TIME);
	  //将限位开关的状态发送给云台
	   fall_down_mileage(&fall_down_move);
		CAN2_Chassis_to_Gimbal();
		//升降设置状态
		fall_down_set_state(&fall_down_move);
		//PID调制
		PID_realize();
			//发送电流给电机
	   if ( toe_is_error(gimbal_can_TOE)||toe_is_error(fall_down_MOTOR1_TOE)|| toe_is_error(fall_down_MOTOR2_TOE))
		{
		CAN_cmd_go_down(0,0,0);
		}	
  	 else
		{	

	   CAN_cmd_go_down(fall_down_move.engineering_fall_down_motor_data[0].give_current,\
	    fall_down_move.engineering_fall_down_motor_data[1].give_current,rescue_IDcard_control.rescue_IDcard_motor_data->give_current);
		}
	}
}
/**
 * @brief 升降任务状态
 * @param engineering_move_state 
 */
static void fall_down_set_state(fall_down_control_t* engineering_move_state)
{	
	switch (engineering_move_state->fall_down_state)
	{
  	
	case In: //如果电机在初始化之后速度都小于定值，那么就当是原点
	
				if( engineering_move_state->fall_down_can_rc->fall_down_speed!=0)
				{
				  
        	 fall_speed=engineering_move_state->fall_down_can_rc->fall_down_speed;		  
					if(fall_speed>fall_down_MAX_SPEED)
					{
					 fall_speed=fall_down_MAX_SPEED;
					}		
						//升降电机上升或下降
					engineering_move_state->engineering_fall_down_motor_data[0].mileage_set -= fall_speed/20;
					engineering_move_state->engineering_fall_down_motor_data[1].mileage_set += fall_speed/20;
					engineering_move_state->engineering_fall_down_motor_data[0].mileage_set = fp32_constrain(engineering_move_state->engineering_fall_down_motor_data[0].mileage_set, -MAX_HIGH, engineering_move_state->fall_down_Init_Angle[0]);

					engineering_move_state->engineering_fall_down_motor_data[1].mileage_set = fp32_constrain(engineering_move_state->engineering_fall_down_motor_data[1].mileage_set, engineering_move_state->fall_down_Init_Angle[1], MAX_HIGH);

				
     	    }
	  

	          if((engineering_move_state->engineering_fall_down_motor_data[1].angle<MAX_HIGH*2/3)&&(engineering_move_state->engineering_fall_down_motor_data[1].angle>MAX_HIGH/4))
						{
							engineering_move_state->Limit_Key_state[0]=ON;
           
						}
            else
            {
              engineering_move_state->Limit_Key_state[0]=OFF;
						
            }
         
           if(engineering_move_state->engineering_fall_down_motor_data[1].angle>=MAX_HIGH-20)
						{
							engineering_move_state->Limit_Key_state[1]=ON;	 
						}
           else
            {        
							engineering_move_state->Limit_Key_state[1]=OFF;	
            }
            
      
	case Act:
	        break;
	case Res:
           

			default:
			break;
	 } 
}

/**
	* @brief          升降任务的初始化
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
void fall_down_Init(fall_down_control_t* engineering_rescue_init)
{

if (engineering_rescue_init == NULL)
	{
		return;
	}
	const static fp32 M3508_fall_down_angle_PID[3]=  {M3508_fall_down_ANGLE_PID_KP, M3508_fall_down_ANGLE_PID_KI, M3508_fall_down_ANGLE_PID_KD};
 	const static fp32 M3508_fall_down_speed_pid[3] = {M3508_fall_down_SPEED_PID_KP, M3508_fall_down_SPEED_PID_KI, M3508_fall_down_SPEED_PID_KD};
    //CAN总线滤波
	const static fp32 engineering_fall_down_order_filter[1] = { engineering_Clamp_turn_NUM };
    //工程开始救援任务的初始状态

	 engineering_rescue_init->fall_down_can_rc=get_can_control_point();

	/**************获取相关电机的指针*************/
	for (int8_t i=0;i<=1;i++)
	{
		engineering_rescue_init->engineering_fall_down_motor_data[i].fall_down_motor_measure = get_fall_down_motor_measure_point(i);
	}

	//PID初始化
    PID_init(&engineering_rescue_init->engineering_fall_down_motor_angle_pid[0], PID_POSITION, M3508_fall_down_angle_PID, M3508_fall_down_ANGLE_PID_MAX_OUT,
		M3508_fall_down_ANGLE_PID_MAX_IOUT);
	PID_init(&engineering_rescue_init->engineering_fall_down_motor_angle_pid[1], PID_POSITION, M3508_fall_down_angle_PID, M3508_fall_down_ANGLE_PID_MAX_OUT,
		M3508_fall_down_ANGLE_PID_MAX_IOUT);

	PID_init(&engineering_rescue_init->engineering_fall_down_motor_pid[0], PID_POSITION, M3508_fall_down_speed_pid, M3508_fall_down_SPEED_PID_MAX_OUT,
		M3508_fall_down_SPEED_PID_MAX_IOUT);
	PID_init(&engineering_rescue_init->engineering_fall_down_motor_pid[1], PID_POSITION, M3508_fall_down_speed_pid, M3508_fall_down_SPEED_PID_MAX_OUT,
		M3508_fall_down_SPEED_PID_MAX_IOUT);
	
	//用一阶滤波代替斜波函数生成
  fall_down_mileage(engineering_rescue_init);       //获取初始
  osDelay(200);
	for(uint8_t i=0;i<2;i++)
	{
	engineering_rescue_init->fall_down_Init_Angle[i]=engineering_rescue_init->engineering_fall_down_motor_data[i].angle;
	}
	first_order_filter_init(&engineering_rescue_init->engineering_cmd_slow_set_fall_down, engineering_CONTROL_TIME, engineering_fall_down_order_filter);
	//最大的移动速度
	engineering_rescue_init->fall_down_motor_max_speed = fall_down_MAX_SPEED;
	engineering_rescue_init->Limit_Key_state[0]=OFF; //限位开关状态初始化
	engineering_rescue_init->Limit_Key_state[1]=OFF; //限位开关状态初始化
	engineering_rescue_init->fall_down_state=In;     //初始默认为运行
}

/**
	* @brief          PID实现
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void PID_realize(void)
{

    fall_down_move.engineering_fall_down_motor_data[0].target_speed
	    =PID_calc(&fall_down_move.engineering_fall_down_motor_angle_pid[0],fall_down_move.engineering_fall_down_motor_data[0].angle,fall_down_move.engineering_fall_down_motor_data[0].mileage_set);

	fall_down_move.engineering_fall_down_motor_data[0].give_current
		= PID_calc(&fall_down_move.engineering_fall_down_motor_pid[0], fall_down_move.engineering_fall_down_motor_data[0].fall_down_motor_measure->speed_rpm, fall_down_move.engineering_fall_down_motor_data[0].target_speed);


    fall_down_move.engineering_fall_down_motor_data[1].target_speed
	    =PID_calc(&fall_down_move.engineering_fall_down_motor_angle_pid[1],fall_down_move.engineering_fall_down_motor_data[1].angle,fall_down_move.engineering_fall_down_motor_data[1].mileage_set);
	fall_down_move.engineering_fall_down_motor_data[1].give_current
		= PID_calc(&fall_down_move.engineering_fall_down_motor_pid[1], fall_down_move.engineering_fall_down_motor_data[1].fall_down_motor_measure->speed_rpm, fall_down_move.engineering_fall_down_motor_data[1].target_speed);
}
/**
	* @brief          上升下降里程距
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void fall_down_mileage(fall_down_control_t* engineering_move_state)
{
engineering_move_state->engineering_fall_down_motor_data[0].angle=(engineering_move_state->engineering_fall_down_motor_data[0].fall_down_motor_measure->ecd_count*8191+engineering_move_state->engineering_fall_down_motor_data[0].fall_down_motor_measure->ecd)*MOTOR_ECD_TO_ANGLE;

engineering_move_state->engineering_fall_down_motor_data[1].angle=(engineering_move_state->engineering_fall_down_motor_data[1].fall_down_motor_measure->ecd_count*8191+engineering_move_state->engineering_fall_down_motor_data[1].fall_down_motor_measure->ecd)*MOTOR_ECD_TO_ANGLE;

}


/***********************************************the end*********************************************/
